/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-09-20     wbj       the first version
 */
#include <rtthread.h>
#include <rtdevice.h>

#define PWM_DEV_NAME "pwm3"
#define PWM_DEV_CHANNEL1  1
#define PWM_DEV_CHANNEL2  2
#define PWM_DEV_CHANNEL3  3
#define PWM_DEV_CHANNEL4  4

struct rt_device_pwm *pwm_dev;

int SG90_WriteChannelAngle_1(float Angle)
{
    rt_uint32_t period,pulse;
    period = 25000000;    /* 周期为0.5ms，单位为纳秒ns */
    pulse=Angle/180*2500000+625000;
        /* 查找设备 */
        pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
        if (pwm_dev == RT_NULL)
        {
            rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
            return RT_ERROR;
        }

        /* 设置PWM周期和脉冲宽度默认值 */
        rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL1, period, pulse);
        /* 使能设备 */
        rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL1);
        return RT_EOK;
}
int SG90_WriteChannelAngle_2(float Angle)
{
    rt_uint32_t period,pulse;
    period = 25000000;    /* 周期为0.5ms，单位为纳秒ns */
    pulse=Angle/180*2500000+625000;
        /* 查找设备 */
        pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
        if (pwm_dev == RT_NULL)
        {
            rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
            return RT_ERROR;
        }

        /* 设置PWM周期和脉冲宽度默认值 */
        rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL2, period, pulse);
        /* 使能设备 */
        rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL2);
        return RT_EOK;
}
int SG90_WriteChannelAngle_3(float Angle)
{
    rt_uint32_t period,pulse;
    period = 25000000;    /* 周期为0.5ms，单位为纳秒ns */
    pulse=Angle/180*2500000+625000;
        /* 查找设备 */
        pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
        if (pwm_dev == RT_NULL)
        {
            rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
            return RT_ERROR;
        }

        /* 设置PWM周期和脉冲宽度默认值 */
        rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL3, period, pulse);
        /* 使能设备 */
        rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL3);
        return RT_EOK;
}
int SG90_WriteChannelAngle_4(float Angle)
{
    rt_uint32_t period,pulse;
    period = 25000000;    /* 周期为0.5ms，单位为纳秒ns */
    pulse=Angle/180*2500000+625000;
        /* 查找设备 */
        pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
        if (pwm_dev == RT_NULL)
        {
            rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
            return RT_ERROR;
        }

        /* 设置PWM周期和脉冲宽度默认值 */
        rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL4, period, pulse);
        /* 使能设备 */
        rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL4);
        return RT_EOK;
}
void woshou(void)
{
    rt_uint8_t i;
    for(i=0;i<3;i++)
    {
        SG90_WriteChannelAngle_1(65);
        rt_thread_mdelay(500);
        SG90_WriteChannelAngle_1(0);
        rt_thread_mdelay(500);
    }
}
void qianjin(void)
{
        SG90_WriteChannelAngle_1(65);
        SG90_WriteChannelAngle_2(65);
        SG90_WriteChannelAngle_3(65);
        SG90_WriteChannelAngle_4(65);
        rt_thread_mdelay(200);
        SG90_WriteChannelAngle_2(95);
        rt_thread_mdelay(200);
        SG90_WriteChannelAngle_3(30);
        rt_thread_mdelay(200);
        SG90_WriteChannelAngle_1(30);
        rt_thread_mdelay(200);
        SG90_WriteChannelAngle_4(95);
        rt_thread_mdelay(200);
}
void paxia(void)
{
    SG90_WriteChannelAngle_1(0);
    SG90_WriteChannelAngle_2(130);
    SG90_WriteChannelAngle_3(130);
    SG90_WriteChannelAngle_4(0);
}
void zhanhao(void)
{
    SG90_WriteChannelAngle_1(65);
    SG90_WriteChannelAngle_2(65);
    SG90_WriteChannelAngle_3(65);
    SG90_WriteChannelAngle_4(65);
}
void zuoxia(void)
{
    SG90_WriteChannelAngle_1(65);
    SG90_WriteChannelAngle_2(65);
    SG90_WriteChannelAngle_3(0);
    SG90_WriteChannelAngle_4(130);
}
void yaobai(void)
{
    SG90_WriteChannelAngle_1(100);
    SG90_WriteChannelAngle_2(30);
    SG90_WriteChannelAngle_3(100);
    SG90_WriteChannelAngle_4(30);
    rt_thread_mdelay(200);
    SG90_WriteChannelAngle_1(30);
    SG90_WriteChannelAngle_2(100);
    SG90_WriteChannelAngle_3(30);
    SG90_WriteChannelAngle_4(100);
    rt_thread_mdelay(200);

}
